#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <std_msgs/msg/int32_multi_array.hpp>
#include <std_msgs/msg/string.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <zbar.h>
#include "origincar_msg/msg/sign.hpp"
#include <vector>
#include <std_msgs/msg/int32.hpp>

using namespace std::chrono_literals;

class CvDetection : public rclcpp::Node {
public:
    CvDetection() : Node("cv_detection"), qr_scanner_() {
        // 初始化参数
        up = 120;
        left = 50;
        area = 1200;
        right_park = 640;
        up_park = 270;
        task = 0;

        // 配置ZBar只识别QR码
        qr_scanner_.set_config(zbar::ZBAR_QRCODE, zbar::ZBAR_CFG_ENABLE, 1);

        // 订阅者
        sign4return_sub_ = create_subscription<std_msgs::msg::Int32>(
            "/sign4return", 10,
            [this](const std_msgs::msg::Int32::SharedPtr msg) { sign4return_callback(msg); });
        
        image_sub_ = create_subscription<sensor_msgs::msg::Image>(
            "/image", 3,
            [this](const sensor_msgs::msg::Image::SharedPtr msg) { image_callback(msg); });

        // 发布者
        cone_pub_ = create_publisher<std_msgs::msg::Int32MultiArray>("/cone", 10);
        park_pub_ = create_publisher<std_msgs::msg::Int32MultiArray>("/park", 10);
        qrcode_pub_ = create_publisher<std_msgs::msg::String>("/qrcode", 10);
        sign_pub_ = create_publisher<origincar_msg::msg::Sign>("sign_switch", 10);
    }

private:
    // 回调函数
    void sign4return_callback(const std_msgs::msg::Int32::SharedPtr msg) {
        if (msg->data == 6) {
            task = 3;
            task3_time = now();
        } else if (msg->data == -1) {
            task = 1;
        } else if (msg->data == 5) {
            task = 2;
        }
    }

    void image_callback(const sensor_msgs::msg::Image::SharedPtr msg) {
        // cv_bridge::CvImagePtr cv_ptr;
        // try {
        //     cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
        // } catch (cv_bridge::Exception& e) {
        //     RCLCPP_ERROR(get_logger(), "cv_bridge exception: %s", e.what());
        //     return;
        // }
        // cv::Mat frame = cv_ptr->image;

        // 将ROS压缩图像消息数据转换为std::vector
        std::vector<uint8_t> data(msg->data.begin(), msg->data.end());

        // 使用OpenCV的imdecode函数解码JPEG图像
        cv::Mat frame = cv::imdecode(data, cv::IMREAD_COLOR);


        if (task == 1) {
            auto cone_pos = cone_frame(frame);
            auto cone_msg = std_msgs::msg::Int32MultiArray();
            cone_msg.data = {cone_pos[0], cone_pos[1], cone_pos[2], cone_pos[3]};
            cone_pub_->publish(cone_msg);

            decode_qr_code(frame);
        }

        if (task == 3) {
            //auto dt = (now() - task3_time).seconds();
            auto dt = (now() - task3_time).nanoseconds() / 1e9; // 更精确的时间计算
            if (dt < 4.0) {
                auto cone_pos = cone_frame(frame);
                auto cone_msg = std_msgs::msg::Int32MultiArray();
                cone_msg.data = {cone_pos[0], cone_pos[1], cone_pos[2], cone_pos[3]};
                cone_pub_->publish(cone_msg);
            } else {
                auto park_pos = park_frame(frame);
                auto park_msg = std_msgs::msg::Int32MultiArray();
                park_msg.data = {park_pos[0], park_pos[1], park_pos[2], park_pos[3]};
                park_pub_->publish(park_msg);
            }
        }
    }

    // 图像处理函数
    std::array<int, 4> cone_frame(cv::Mat& frame) {
        cv::Mat roi = frame(cv::Range(up, 450), cv::Range(left, 600));
        cv::Mat hsv;
        cv::cvtColor(roi, hsv, cv::COLOR_BGR2HSV);

        cv::Mat mask1, mask2;
        cv::inRange(hsv, cv::Scalar(0, 0, 0), cv::Scalar(82, 255, 255), mask1);
        cv::inRange(hsv, cv::Scalar(0, 0, 0), cv::Scalar(255, 90, 255), mask2);
        cv::bitwise_or(mask1, mask2, mask1);

        std::vector<std::vector<cv::Point>> contours;
        cv::findContours(255 - mask1, contours, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);

        int max_area = 0;
        cv::Rect largest_rect;
        for (auto& cnt : contours) {
            int area = cv::contourArea(cnt);
            if (area > this->area) {
                cv::Rect rect = cv::boundingRect(cnt);
                if (area > max_area) {
                    max_area = area;
                    largest_rect = rect;
                }
            }
        }

        return {
            int (left + largest_rect.x + largest_rect.width/2),
            up + largest_rect.y + largest_rect.height,
            largest_rect.width,
            largest_rect.height
        };
    }

    std::array<int, 4> park_frame(cv::Mat& frame) {
        cv::Mat roi = frame(cv::Range(up_park, 450), cv::Range(0, right_park));
        cv::Mat hsv;
        cv::cvtColor(roi, hsv, cv::COLOR_BGR2HSV);

        cv::Mat mask1, mask2;
        cv::inRange(hsv, cv::Scalar(0, 0, 0), cv::Scalar(47, 73, 255), mask1);
        cv::inRange(hsv, cv::Scalar(0, 0, 0), cv::Scalar(255, 50, 255), mask2);
        cv::bitwise_and(mask1, 255-mask2, mask1);

        std::vector<std::vector<cv::Point>> contours;
        cv::findContours(mask1, contours, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);

        int max_area = 0;
        cv::Rect largest_rect;
        for (auto& cnt : contours) {
            int area = cv::contourArea(cnt);
            if (area > this->area) {
                cv::Rect rect = cv::boundingRect(cnt);
                if (area > max_area) {
                    max_area = area;
                    largest_rect = rect;
                }
            }
        }

        return {
            int (largest_rect.x + largest_rect.width/2),
            int (up_park + largest_rect.y + largest_rect.height/2),
            largest_rect.width,
            largest_rect.height
        };
    }

    // QR码解码
    void decode_qr_code(cv::Mat& frame) {
        // zbar::ImageScanner scanner;
        // cv::Mat gray;
        // cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);
        zbar::Image image(frame.cols, frame.rows, "Y800", frame.data, frame.cols * frame.rows);
        
        // scanner.scan(image);
        // for (auto s = image.symbol_begin(); s != image.symbol_end(); ++s) {
        //     std::string qr_code_data = s->get_data();
        //     if (qr_code_data == "ClockWise" || qr_code_data == "AntiClockWise") {
        //         publish_sign_message(qr_code_data);
        //         return; // 成功识别后返回

        //     }
        // }
        if (qr_scanner_.scan(image)) {
            for (auto s = image.symbol_begin(); s != image.symbol_end(); ++s) {
                std::string data = s->get_data();
                if (data == "ClockWise" || data == "AntiClockWise") {
                    publish_sign_message(data);
                    return;
                }
            }
        }
    }

    void publish_sign_message(const std::string& qr_code_data) {
        origincar_msg::msg::Sign sign_msg;
        sign_msg.sign_data = (qr_code_data == "ClockWise") ? 3 : 4;
        sign_pub_->publish(sign_msg);
        RCLCPP_INFO(this->get_logger(), "已发布二维码信息: %s", qr_code_data.c_str());

        auto msg = std_msgs::msg::String();
        msg.data = qr_code_data;
        qrcode_pub_->publish(msg);
        task = 2;
    }

    // 变量声明
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr sign4return_sub_;
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
    rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr cone_pub_;
    rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr park_pub_;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr qrcode_pub_;
    rclcpp::Publisher<origincar_msg::msg::Sign>::SharedPtr sign_pub_;
    zbar::ImageScanner qr_scanner_; // 二维码扫描器
    int up, left, area, right_park, up_park;
    int task = 0;
    rclcpp::Time task3_time;
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<CvDetection>();
    //
    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(node);
    executor.spin();
    //
    // rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}